                               kl------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------package pteam;

import java.util.Vector;

public class RangeFinderAnalysis {
  public static final double DANGERDISTANCE = 1.0;
  public static final double GAPTOLERANCE=8000.0;  //TODO figure tolerance for gap and plateau
  public static final double PLATTOLERANCE=5.0;
  public static final double SUBSIDETOLERANCE=5.0;
  public static final double DEVIATION_THRESHOLD=20000;
  public static final double AVERAGE_DISTANCE_THRESHOLD=10.0;
  public static double[] normalGraph;
  public static boolean risingOrFalling(boolean rising, double[][] sensorstates) {
    // sensors states is a HISTORYSIZE(3) x2
    if (sensorstates[0][0] < 1 || sensorstates[1][0] < 1 || sensorstates[2][0] < 1 || SimpleNavigator.adjusting)
      return false;
    double deltaY1 = sensorstates[1][0] - sensorstates[0][0];
    double deltaT1 = sensorstates[1][1] - sensorstates[0][1];
    double deltaY2 = sensorstates[2][0] - sensorstates[1][0];
    double deltaT2 = sensorstates[2][1] - sensorstates[1][1];
    //Simulator.out.println("timing = " + sensorstates[0][1] + " " + sensorstates[1][1] + " " + sensorstates[2][1]);
    //Simulator.out.println("states = " + sensorstates[0][0] + " " + sensorstates[1][0] + " " + sensorstates[2][0]);
    
    double slope1 = deltaY1 / deltaT1;
    double slope2 = deltaY2 / deltaT2;

    double size= (slope2 - slope1) / (sensorstates[2][1] - sensorstates[0][1]);
    
    boolean result;
    result = (Math.abs(size) >= GAPTOLERANCE);
    
    //Simulator.out.println((result?"(detected a gap) ":"(no gap detected) ") + slope1 + " " + slope2 + " " + size);
    return result;
  }

  /**
   * Given a direction to rotate in and an absolute direction,
   * 
   */
  public static int rotateDirection(int direction, boolean clockwise) {
    if(clockwise){
      switch(direction){
        case Tree.NORTH:
          return Tree.EAST;
        case Tree.SOUTH:
          return Tree.WEST;
        case Tree.EAST:
          return Tree.SOUTH;
        case Tree.WEST:
          return Tree.NORTH;
        default:
          throw new RuntimeException("This will never happen! (rotating a non-direction)");
      }
    }else {
      switch(direction){
        case Tree.SOUTH:
          return Tree.EAST;
        case Tree.NORTH:
          return Tree.WEST;
        case Tree.EAST:
          return Tree.NORTH;
        case Tree.WEST:
          return Tree.SOUTH;
        default:
          throw new RuntimeException("This will never happen! (rotating in a non-direction)");
      }
    }
  }
  
  public static void initNormal(double frontRange, double[] rangeData){
	  normalGraph = new double[rangeData.length];
	  for(int i = 0; i < rangeData.length; i++) {
		  normalGraph[i] = rangeData[i]/(Math.cos(Math.toRadians(i))*frontRange);
	  }
  }
  
  public static Vector objectIn(double frontRange, double[] rangeData)
  {
	  Vector v = new Vector();
	  for(int i=0; i<rangeData.length; i++){
		  v.addElement(new DeviantGap(i, i, rangeData[i]/(Math.cos(Math.toRadians(i))*frontRange)));
	  }
	  
	  //double[] normalGraph = new double[rangeData.length];
	  /*double multiplier = 1;
	  int devLength = 0, devTotal = 0, devStart = -1;
	  boolean sideChange;
	  for(int i = 0;i < normalGraph.length; i++)
	  {
		   System.out.println(i + " " + normalGraph[i]*(Math.cos(Math.toRadians(i))*frontRange) + " " + rangeData[i]);
		  sideChange = false;//(normalGraph[i]*frontRange - rangeData[i]) * (normalGraph[(i-1 >= 0? i-1 : 0)] - rangeData[(i-1 >= 0? i-1 : 0)]) < 0;
		  if((Math.abs(normalGraph[i]*(Math.cos(Math.toRadians(i))*frontRange) - rangeData[i]) > DEVIATION_THRESHOLD && !sideChange) || rangeData.length -1 ==i)
		  {
			  devTotal += normalGraph[i]*(Math.cos(Math.toRadians(i))*frontRange) - rangeData[i];
			  devLength++;
			  if(devStart == -1)
				  devStart = i;
		  } else if(devLength>0){
			  v.addElement(new DeviantGap(devStart, i, devTotal/devLength));
			  devStart = -1;
			  devTotal = 0;
			  devLength = 0;
			  if(sideChange)
			  {
				  devTotal += normalGraph[i] - rangeData[i];
				  devLength++;
				  if(devStart == -1)
					  devStart = i;
			  }
		  }
	  }*/
	  return v;
  }
}